import sys
sys.path.insert(0, "/home/ninetripod/catkin_ws/src/uav/scripts")
sys.path.insert(0, "/home/ninetripod/catkin_ws/src/uav/scripts/sensor")
# print(sys.path)

import rospy
from nav_msgs.msg import Odometry
from std_msgs.msg import String

import serial
from peripherals import get_LED2
from fix import Coord_fix, Projection

from time import time
import json

class T265_Subscriber():
    offset = [-3.2, 1.3, 4.2]
    def __init__(self, port = '/dev/ttyS0'):
        # 数据
        self.pos = None
        self.vel = None

        # 发送串口
        self._ser = serial.Serial(port, 115200)

        self._scale = 1000
        self._count = 0

        self._fixer = None

        # debug
        self.proj = None

        self.pub = rospy.Publisher("uav/pos", String, queue_size = 10)

        # 订阅t265消息
        rospy.Subscriber(
            "/camera/odom/sample",
            Odometry, 
            self.callback
        )

        self.update_time = time()


    def callback(self, data: Odometry):
        if self._count == 10:
            self.orient = data.pose.pose.orientation
            self.pos = data.pose.pose.position
            self.vel = data.twist.twist.linear

            if self._fixer is None:
                self._fixer = Coord_fix(self.orient, T265_Subscriber.offset)

            pos_fixed = self._fixer.pos_fix(self.orient, self.pos)
            msg = self._form_data(pos_fixed, self.vel)

            # # debug
            # if self.proj is None:
            #     quat = [self.orient.w, self.orient.x, self.orient.y, self.orient.z]
            #     self.proj = Projection(quat)
            # else:
            #     quat = [self.orient.w, self.orient.x, self.orient.y, self.orient.z]
            #     self.proj.pre_transform(quat,120)
            #     pos = self.proj.transform([0,-50])
            #     print('\rx:{},y:{}'.format(pos[0],pos[1]),end='', flush=True)

            # print(str(pos_fixed))
            # print(msg)
            # pos_fixed.printv()


            self._ser.write(msg)

            self.publish(pos_fixed, self.orient)


            self._count = 0
            self.update_time = time()

        else:
            self._count += 1

    def _form_data(self, pos, vel):
        # print(pos)

        p_x = int(pos.x *10).to_bytes(2, 'big', signed = True)
        p_y = int(pos.y *10).to_bytes(2, 'big', signed = True)
        p_z = int(pos.z *10).to_bytes(2, 'big', signed = True)

        v_x = int(vel.x *self._scale).to_bytes(2, 'big', signed = True)
        v_y = int(vel.y *self._scale).to_bytes(2, 'big', signed = True)
        v_z = int(vel.z *self._scale).to_bytes(2, 'big', signed = True)
        
        msg = [0xFE, 
               p_x[0], p_x[1], p_y[0], p_y[1], p_z[0], p_z[1], 
               v_x[0], v_x[1], v_y[0], v_y[1], v_z[0], v_z[1], 
               0x00]
        # print(msg)
        return msg
    
    def check_status(self, timer):
        if time() - self.update_time > 8:
            led.write(False)
            print("t265 offline")
            rospy.signal_shutdown("t265 timeout")


    def publish(self, pos, orient):
        data = {
            "p": [pos.x ,pos.y, pos.z], 
            "o": [orient.w, orient.x, orient.y, orient.z]
        }
        self.pub.publish(json.dumps(data))



if __name__ == '__main__':
    led = get_LED2()

    try:
        rospy.init_node("t265_subscriber")
        t265_sub = T265_Subscriber()

        check_timer = rospy.timer.Timer(
            rospy.Duration(2),
            t265_sub.check_status
        )

        led.write(True)

        print("t265 subscriber init done")

        rospy.spin()

    finally:
        led.write(False)
        led.close()
